#include <iostream>
#include <random>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include "common/timer.hpp"
#include "pcl_fitting.hpp"

DEFINE_int32(num_tested_points_plane,10,"number of tested points in plane fitting");
DEFINE_double(noise_sigma, 0.01, "noise of generated samples");

void PlaneFittingTest();

int main(int argc,char** argv)
{
    google::SetUsageMessage("This program tests plane fitting algorithms.");
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    LOG(INFO) << "testing plane fitting";
    PlaneFittingTest();
    return 0;
}

void PlaneFittingTest()
{
    //真实平面系数
    Eigen::Vector4d true_plane_coeffs(0.1,0.2,0.3,0.4);
    true_plane_coeffs.normalize();

    std::vector<Eigen::Vector3d> points;
    //随机数生成器
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<double> uniform_dist(0.0,1.0);//均匀分布
    std::normal_distribution<double> normal_dist(0.0,FLAGS_noise_sigma);//正态分布
    //生成平面点并添加噪声
    for(int i=0;i<FLAGS_num_tested_points_plane;++i)
    {
        //生成随机点
        Eigen::Vector3d p(
            uniform_dist(gen),
            uniform_dist(gen),
            uniform_dist(gen)
        );

        //计算平面的第四维分量，再归一化到平面，最后添加噪声
        double n4=-p.dot(true_plane_coeffs.head<3>())/true_plane_coeffs[3];
        p=p/(n4+std::numeric_limits<double>::min());//防止除零

        //添加高斯噪声
        p+=Eigen::Vector3d{
            normal_dist(gen),
            normal_dist(gen),
            normal_dist(gen)
        };
        points.push_back(p);
        // 验证点是否在平面上
        //  Debug 日志：输出输入点数量（VLOG(1)，较低优先级的调试信息）
        VLOG(1) << "[Debug] Plane_FittingTest: residual of p= " << p.dot(true_plane_coeffs.head<3>()) + true_plane_coeffs[3];
        // LOG(INFO) << "residual of p: " << p.dot(true_plane_coeffs.head<3>()) + true_plane_coeffs[3];
    }

    //拟合平面
    Eigen::Vector4d estimated_plane_coeffs;

    std::chrono::steady_clock::time_point start;
    timer_begin("FiltPlane-SVD");
    if (pclTool::FitPlane(points, estimated_plane_coeffs, 1e-2, pclTool::solveMethod::SVD))
    {
        timer_end("FiltPlane-SVD");
        LOG(INFO) << "estimated coeffs: " << estimated_plane_coeffs.transpose()
                  << ", true: " << true_plane_coeffs.transpose();
                  LOG(INFO)<<"=================================";
    }
    else
    {
        LOG(INFO) << "plane fitting failed";
        LOG(INFO)<<"==================================";
    }

    timer_begin("FiltPlane-QR");
    if (pclTool::FitPlane(points, estimated_plane_coeffs, 1e-2, pclTool::solveMethod::QR))
    {
        timer_end("FiltPlane-QR");
        LOG(INFO) << "estimated coeffs: " << estimated_plane_coeffs.transpose()
                  << ", true: " << true_plane_coeffs.transpose();
        LOG(INFO) << "=================================";
    }
    else
    {
        LOG(INFO) << "plane fitting failed";
        LOG(INFO) << "==================================";
    }
}